//
// Created by Hoyin on 2023/12/25.
//

#include "../include/Coordinate.h"

void Coordinate::applyShift(Matrix<double, 3, 1> V) {
    XYZ = XYZ + V;
}

double Coordinate::X() const {
	return XYZ[0];
}

double Coordinate::Y() const {
	return XYZ[1];
}

double Coordinate::Z() const {
	return XYZ[2];
}

Vector3d Coordinate::getXYZ() const {
	return XYZ;
}

Matrix3d Rotation::getRotationMatrix(double phi, double omega, double kappa) {
    Matrix3d Rphi, Romega, Rkappa;
    Rphi <<
         cos(phi), 0, -sin(phi),
         0, 1, 0,
		 sin(phi), 0, cos(phi);

	Romega <<
		1, 0, 0,
		0, cos(omega), -sin(omega),
		0, sin(omega), cos(omega);

	Rkappa <<
		cos(kappa), -sin(kappa), 0,
		sin(kappa), cos(kappa), 0,
		0, 0, 1;

	Matrix3d R = Rphi*Romega*Rkappa;
	return R;
}

void Rotation::setRotation(double phi, double omega, double kappa) {
	R = getRotationMatrix(phi, omega, kappa);
}

void Rotation::setRotation(Eigen::Matrix3d R) {
	this->R = R;
}

